/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package control;

import Jama.Matrix;
import plot.GNUPlot;

/**
 *
 * @author dheath
 */
public class Kalman 
{
    private Matrix F;
    private Matrix H;
    private Matrix Ut;
    private Matrix Et;
    private Matrix Ex;
    private Matrix Ez;
    
    private double noise;
    
    public Kalman(double noise)
    {
        this.noise = noise;
        
        resetFilter(noise);
        
    }
    
    public Matrix getFuturePrediction(double dT)
    {
        setF(dT);
        //Matrix P = F.times(Ut);
        
        return F.times(Ut);
    }
    public void plotFuturePosition(GNUPlot plot, double x, double y) {
    	double sig_x = Math.sqrt(Et.get(0,0)); //stdevx
        double sig_y = Math.sqrt(Et.get(3,3)); //stdevy
        double rho = 0;
        if(sig_x * sig_y != 0) rho = Et.get(0,3) / (sig_x * sig_y);
    	plot.plotFuturePosition(x, y, sig_x, sig_y, rho);
    }
    public Matrix updateFilter(Matrix Zt, double dT)
    {
        
        setF(dT);
    
        Matrix Kt = kPart();
        
        Ut = uPart(Kt, Zt);
        Et = Sigpart(Kt);
        
        return Ut;
    }
    public void plotMultivariateNormal(GNUPlot plot, double dT) {
    	double x = Ut.get(0, 0);
        double y = Ut.get(3,0);
        double sig_x = Math.sqrt(Et.get(0,0)); //stdevx
        double sig_y = Math.sqrt(Et.get(3,3)); //stdevy
        double rho = 0;
        if(sig_x * sig_y != 0) rho = Et.get(0,3) / (sig_x * sig_y);
        // I will assume that X and Y are the random variables whose values are
        // observed in the matrix Zt
        // Also, I will assume that Ut holds the data for the mean x and y values.
       /* double X = Zt.get(0,0);
        double Y = Zt.get(1,1);
        double Usubx = Ut.get(0,0);
        double Usuby = Ut.get(3,0);
        double cov = (X - Usubx)*(Y - Usuby);
        double rho = E(cov)/(sig_x * sig_y);*/
        plot.plotMultivariateNormal(x, y, sig_x, sig_y, rho, dT);
    }
    
    // computes the expected value of X=x according to normal distribution...
    private double E(double x) 
    {
       // return x;
        return 0;
    }
    public void resetFilter(double noise)
    {
        double[][] EoEo = {{100, 0,   0,     0,  0,  0},
                          {0,    .1,  0,     0,  0,  0},
                          {0,    0,   .1,    0,  0,  0}, //C = 0
                          {0,    0,   0,     100,0,  0},
                          {0,    0,   0,     0,  .1, 0},
                          {0,    0,   0,     0,  0, .1}};
        Et = new Matrix(EoEo);
        
        double[][] uu = {{0},
                         {0},
                         {0},
                         {0},
                         {0},
                         {0}};
        Ut = new Matrix(uu);
        
        
        
        setF(1);
        
        double[][] ExEx = {{.1, 0,   0,     0, 0, 0},
                          {0,   .5,  0,     0, 0, 0},
                          {0,   0,   20,   0, 0, 0}, //C = 0
                          {0,   0,   0,     .1,0, 0},
                          {0,   0,   0,     0, .5,0},
                          {0,   0,   0,     0, 0, 20}};
        Ex = new Matrix(ExEx);
        
        double[][] hh = {{1, 0,  0, 0, 0, 0},
                         {0, 0,  0, 1, 0, 0}};
        H = new Matrix(hh);
        
        double[][] ez = {{noise,0},
                         {0,noise}};
        Ez = new Matrix(ez);
    }
    
    private void setF(double dT)
    {
        double[][] ff = {{1.0, dT, dT*dT/2,0, 0, 0},
                         {0,   1.0,  dT,     0, 0, 0},
                         {0,   .1,   1.0,     0, 0, 0}, //C = 0
                         {0,   0,   0,     1.0, dT,dT*dT/2},
                         {0,   0,   0,     0, 1.0, dT},
                         {0,   0,   0,     0, .1, 1.0}}; //c = .1
        
        F = new Matrix(ff);
    }
    private Matrix kPart()
    {
        Matrix K = F.times(Et);
        K = K.times(F.transpose());
        K = K.plus(Ex);
        K = K.times(H.transpose());
        
        
        Matrix J = F.times(Et);
        J = J.times(F.transpose());
        J = J.plus(Ex);
        J = J.times(H.transpose());
        
        Matrix L = H.times(J);
        L = L.plus(Ez);
        
        K = K.times(L.inverse());
       
        //K.print(4, 4);
        return K;
    }
    
    private Matrix uPart(Matrix Kt, Matrix Zt1)
    {
        Matrix r = H.times(F).times(Ut);
        r.set(0,0,Zt1.get(0, 0)-r.get(0, 0));
        r.set(1,0,Zt1.get(3, 0)-r.get(1, 0));
        r = Kt.times(r);
        
        Matrix d = F.times(Ut).plus(r);
        
        return d;
    }
    
    private Matrix Sigpart(Matrix Kt)
    {
        Matrix I = Matrix.identity(6, 6);
        
        Matrix res = F.times(Et).times(F.transpose()).plus(Ex);
        
        res = I.minus(Kt.times(H)).times(res);
        
        return res;
    }
    
}
